#include "LocationSoftwareMapInfo.h"
#include <bosch_locator_bridge/ClientControlMode.h>
#include <boost/thread/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <thread>

namespace kybot_map
{

    LocationSoftwareMapInfo::LocationSoftwareMapInfo()
        : MapInfo()
    {
        ros::NodeHandle nh("~");
        laser_puber_ = nh.advertise<sensor_msgs::PointCloud2>("/map_manager_node/scan", 1, true);

        SubscribeTopics();
    }

    void LocationSoftwareMapInfo::SubscribeTopics()
    {
        ros::NodeHandle nh("/");

        status_sub_ = nh.subscribe<bosch_locator_bridge::ClientLocalizationVisualization>("/bridge_node/client_localization_visualization", 1, 
        boost::function<void(bosch_locator_bridge::ClientLocalizationVisualization)>([this](bosch_locator_bridge::ClientLocalizationVisualization client_localization_visualization)
            { 
                status_ = client_localization_visualization.loc_state; 
            }
            ));

        pose_sub_ = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/bridge_node/client_localization_pose/pose", 1, 
        boost::function<void(geometry_msgs::PoseWithCovarianceStamped)>([this](geometry_msgs::PoseWithCovarianceStamped pose)
        {
            tf::Quaternion quat;
            tf::quaternionMsgToTF(pose.pose.pose.orientation, quat);
            double roll, pitch, yaw;
            tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
            pose_.x = pose.pose.pose.position.x;
            pose_.y = pose.pose.pose.position.y;
            pose_.theta = yaw;

        }));

        laser_sub_ = nh.subscribe<sensor_msgs::PointCloud2>("/bridge_node/client_localization_visualization/scan", 1,
                                                                boost::bind(&LocationSoftwareMapInfo::ScanCallback, this, _1));
    }

    void LocationSoftwareMapInfo::ScanCallback(const sensor_msgs::PointCloud2ConstPtr& scan)
    {
        laser_scan_ = *scan;
        laser_puber_.publish(scan); 
    }

}